// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBR4JointPositionController.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.KukaLBR
{
    using System;
    using System.Collections.Generic;
    using System.ComponentModel;

    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.ControllerStateMachine;
    using Microsoft.Robotics.Numerics;

    using LBR4 = Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4PlusCsharpDefinitionsNativeMethods;

    /// <summary>
    /// Controller for joint position control
    /// </summary>
    public class KukaLBR4JointPositionController : KukaLBR4Controller
    {
        /// <summary>
        /// Controller state machine description cache
        /// </summary>
        private static ControllerStateMachineDescription csmdCache;

        /// <summary>
        /// Determines if we have a cached copy of the controller state machine description available
        /// </summary>
        private static bool haveCachedCsmd = false;

        /// <summary>
        /// Register that stores the desired joint position
        /// </summary>
        private RegisterID desiredJointPositionRegister;

        /// <summary>
        /// max joint velocity
        /// </summary>
        private RegisterID maxJointVelocityRegister;

        /// <summary>
        /// cycle time step in millisecond
        /// </summary>
        private RegisterID cycleTimeInMsRegister;

        /// <summary>
        /// Register that stores the result of computing the actual commanded joint
        /// position.  This should be computed by getting the commanded joint position
        /// reported by FRI and applying the offset reported by FRI
        /// </summary>
        private RegisterID commandedJointPositionAtControllerStart;

        /// <summary>
        /// Register that stores the value indicating that FRI is in the command state.
        /// </summary>
        private RegisterID friCmdStateValueRegister;

        /// <summary>
        /// The number of cycles the motion should take.
        /// </summary>
        private RegisterID durationOfMotionInCyclesRegister;

        /// <summary>
        /// The current cycle in the motion.
        /// </summary>
        private RegisterID currentCycleRegister;

        /// <summary>
        /// Register to hold the value one.
        /// </summary>
        private RegisterID positiveOneRegister;

        /// <summary>
        /// Initializes a new instance of the <see cref="KukaLBR4JointPositionController" /> class
        /// </summary>
        /// <param name="registerManager">the register manager</param>
        /// <param name="goal">the desired joint position</param>
        /// <param name="maxJointVelocityInRadPerSec">max joint velocity</param>
        /// <param name="timeStepInMsec">Duration of a single step in ms.</param>
        /// <param name="durationInCycles">Duration of the motion in cycles.</param>
        public KukaLBR4JointPositionController(
            KukaLBR4WorkingRegisterManager registerManager,
            double[] goal,
            double maxJointVelocityInRadPerSec,
            double timeStepInMsec,
            double durationInCycles = -1) :
            base(registerManager)
        {
            // examine the dimension of the input joint goal, which should match the degrees of freedom of the arm
            if (goal.Length != KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS)
            {
                string msg;
                msg = string.Format("the length of the goal should match the degrees of freedom of the robot arm: expected {0} but received {1}", KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS, goal.Length);
                throw new ArgumentException(msg);
            }

            if (durationInCycles <= 1)
            {
                // not possible to reach a goal in less than a single control step
                durationInCycles = 1;
            }

            this.commandedJointPositionAtControllerStart = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.JointVector));

            this.desiredJointPositionRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.JointVector));

            this.maxJointVelocityRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            this.cycleTimeInMsRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            this.friCmdStateValueRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            this.durationOfMotionInCyclesRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            this.currentCycleRegister = RegisterID.NewWorkingRegister(
                this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            this.positiveOneRegister = RegisterID.NewWorkingRegister(
               this.RegisterManager.GetNextRegisterID(KukaLBR4WorkingRegisterManager.RegisterValueTypes.Scalar));

            // CMD state value
            this.WorkingRegisterArray.Registers[this.friCmdStateValueRegister.RegisterNumber][0] = 2;

            // Configure for the number of cycles 
            this.WorkingRegisterArray.Registers[this.positiveOneRegister.RegisterNumber][0] = 1;
            this.WorkingRegisterArray.Registers[this.currentCycleRegister.RegisterNumber][0] = 0;
            this.WorkingRegisterArray.Registers[this.durationOfMotionInCyclesRegister.RegisterNumber][0] = durationInCycles;

            // copy the joint goal to the working register
            Array.Copy(
                goal,
                this.WorkingRegisterArray.Registers[this.desiredJointPositionRegister.RegisterNumber],
                this.WorkingRegisterArray.Registers[this.desiredJointPositionRegister.RegisterNumber].Length);

            this.WorkingRegisterArray.Registers[this.maxJointVelocityRegister.RegisterNumber][0] = maxJointVelocityInRadPerSec;
            this.WorkingRegisterArray.Registers[this.cycleTimeInMsRegister.RegisterNumber][0] = timeStepInMsec;
        }

        /// <summary>
        /// Builds a state that synchronizes joint position with FRI.
        /// </summary>
        /// <param name="commandedJointPositionAtControllerStartRegister"> Register to hold the joint position command at controller start. </param>
        /// <returns>A controller state machine state.</returns>
        public static State BuildSynchronizeFRIJointPositionState(RegisterID commandedJointPositionAtControllerStartRegister)
        {
            ControllerOperation[] operationsList = new ControllerOperation[2];

            // Operation 0
            // Add operation (CopyVec7Op) to copy current measured commanded position (based on last command issued
            // to FRI) to the starting position that will be used for the move command in state 1.
            // We do this here (rather than use the LastIssuedJointPositionCommandRegister directly in state 1)
            // because we want the initial position to remain constant over succsive control iterations so that new
            // joint references are properly computed.
            RegisterID[] operation0Operands = new RegisterID[1];

            operation0Operands[0] =
                new RegisterID(
                    RegisterTypes.Feedback,
                    (int)LBR4.KukaLBR4PlusFeedbackRegisters.LastIssuedJointPositionCommandRegister);

            // copy current joint position to command register
            operationsList[0] = new ControllerOperation(
                (int)LBR4.KukaLBR4Operations.CopyVec7Op,
                commandedJointPositionAtControllerStartRegister,
                operation0Operands);

            // operation 1 - calculate the current target and set the commanded joint target to matching values
            // Get the current command by adding the commanded joint position (CommandedJointPositionRegister) and
            // the commanded joint offset (CommandedJointOffsetRegister)
            // refer to page 33 in http://sharepoint/sites/Baker/SiteAssets/Baker%20Wiki/LBR4+%20Manuals/KUKA_FRI_10_en.pdf
            // for the math behind and protocol reasons this is required.
            RegisterID[] operation1Operands = new RegisterID[2];
            operation1Operands[0] = new RegisterID(
                RegisterTypes.Feedback,
                (int)LBR4.KukaLBR4PlusFeedbackRegisters.CommandedJointPositionRegister);
            operation1Operands[1] = new RegisterID(
                RegisterTypes.Feedback,
                (int)LBR4.KukaLBR4PlusFeedbackRegisters.CommandedJointOffsetRegister);

            operationsList[1] = new ControllerOperation(
                (int)LBR4.KukaLBR4Operations.AddVec7Op,
                 new RegisterID(
                    RegisterTypes.Working,
                    (int)LBR4.KukaLBR4PlusWorkingRegisters.JointPositionCommandRegister),
                operation1Operands);

            return new State(
                (int)LBR4.ControlModes.JointPositionControlMode,
                operationsList,
                new Transition[0],
                new Transition[0]);
        }

        /// <summary>
        /// Builds a state machine controller
        /// </summary>
        /// <returns>A state machine based controller</returns>
        public new ControllerStateMachineDescription BuildStateMachineDescription()
        {
            if (!haveCachedCsmd)
            {
                csmdCache = base.BuildStateMachineDescription();
                haveCachedCsmd = true;
            }

            return csmdCache;
        }

        /// <summary>
        /// Builds a state that generates a small step in joint space towards our goal
        /// </summary>
        /// <param name="initialJointPositionCommandRegister">currently commanded joint positions.</param>
        /// <param name="desiredJointPositionRegister">joint goal</param>
        /// <param name="maxJointVelocityRegister">maximum joint velocity in rad/sec</param>
        /// <param name="cycleTimeInMsRegister">fri cycle time</param>
        /// <param name="durationOfMotionInCyclesRegister">Register holding the duration of the motion in cycles.</param>
        /// <param name="currentCycleRegister">Register holding the number of cycles the motion has been active for.</param>
        /// <param name="cycleIncrementRegister">Register holding a value indicating how much to increment the cycle count by on each pass.</param>
        /// <returns>a controller state machine state</returns>
        protected static State BuildSimpleJointInterpolationState(
            RegisterID initialJointPositionCommandRegister,
            RegisterID desiredJointPositionRegister,
            RegisterID maxJointVelocityRegister,
            RegisterID cycleTimeInMsRegister,
            RegisterID durationOfMotionInCyclesRegister,
            RegisterID currentCycleRegister,
            RegisterID cycleIncrementRegister)
        {
            RegisterID jointPositionCommandRegister = new RegisterID(
                RegisterTypes.Working,
                (int)LBR4.KukaLBR4PlusWorkingRegisters.JointPositionCommandRegister);

            ControllerOperation[] operationList = new ControllerOperation[2];

            // operation 0 - increment cycle count
            RegisterID[] incrementCycleCountOperands = new RegisterID[2];
            incrementCycleCountOperands[0] = currentCycleRegister;
            incrementCycleCountOperands[1] = cycleIncrementRegister;

            operationList[0] = new ControllerOperation(
               (int)LBR4.KukaLBR4Operations.AddOp,
               currentCycleRegister,
               incrementCycleCountOperands);

            // operation 1 - interpolate
            // we interpolate using
            // pre-defined interpolation function (SimpleJointInterpolationOp) to compute the next
            // joint position for the control time step
            RegisterID[] simpleInterpolationOperands = new RegisterID[6];
            simpleInterpolationOperands[0] = initialJointPositionCommandRegister;
            simpleInterpolationOperands[1] = desiredJointPositionRegister;
            simpleInterpolationOperands[2] = maxJointVelocityRegister;
            simpleInterpolationOperands[3] = cycleTimeInMsRegister;
            simpleInterpolationOperands[4] = durationOfMotionInCyclesRegister;
            simpleInterpolationOperands[5] = currentCycleRegister;

            operationList[1] = new ControllerOperation(
                (int)LBR4.KukaLBR4Operations.SimpleJointInterpolationOp,
                jointPositionCommandRegister,
                simpleInterpolationOperands);

            return new State(
                (int)LBR4.ControlModes.JointPositionControlMode,
                operationList,
                new Transition[0],
                new Transition[0]);
        }

        /// <summary>
        /// Builds the controller state machine used by this controller.
        /// </summary>
        /// <returns>A controller state machine.</returns>
        protected override ControllerStateMachine BuildControllerStateMachine()
        {
            const int SynchronizationState = 0;
            const int JointInterpolationState = 1;

            ControllerStateMachine csm = new ControllerStateMachine();

            State[] stateArray = new State[2];

            stateArray[SynchronizationState] =
                BuildSynchronizeFRIJointPositionState(this.commandedJointPositionAtControllerStart);

            stateArray[JointInterpolationState] =
                BuildSimpleJointInterpolationState(
                initialJointPositionCommandRegister:
                this.commandedJointPositionAtControllerStart,
                desiredJointPositionRegister:
                this.desiredJointPositionRegister,
                maxJointVelocityRegister:
                this.maxJointVelocityRegister,
                cycleTimeInMsRegister:
                this.cycleTimeInMsRegister,
                durationOfMotionInCyclesRegister:
                this.durationOfMotionInCyclesRegister,
                currentCycleRegister:
                this.currentCycleRegister,
                cycleIncrementRegister:
                this.positiveOneRegister);

            csm.States = new List<State>(stateArray);

            // create a transition from the synchronization state to the interpolation state when
            // the state of the FRI connection reaches the desired value in Scalar1 (typically 2,
            // as this indicates command state -- a FRI error will result if we transition to the
            // interpolation state when the FRI connection is not in the CMD state).
            RegisterID[] transitionOnFriCmdOperands = new RegisterID[2];
            transitionOnFriCmdOperands[0] =
                this.friCmdStateValueRegister;
            transitionOnFriCmdOperands[1] =
                RegisterID.NewFeedbackRegister((int)LBR4.KukaLBR4PlusFeedbackRegisters.FRIStateRegister);

            csm.States[SynchronizationState].TransitionsToEvaluateAfterDefaults = new Transition[1];

            csm.States[SynchronizationState].TransitionsToEvaluateAfterDefaults[0] = new Transition(
                csm.States[JointInterpolationState],
                (int)LBR4.KukaLBR4Operations.AreScalarsEqualOp,
                transitionOnFriCmdOperands,
                new ControllerOperation[0]);

            return csm;
        }
    }
}
